package packageLego;

import lejos.nxt.Button;
import lejos.nxt.LCD;
import lejos.nxt.Motor;
import lejos.nxt.SensorPort;
import lejos.nxt.TouchSensor;
import lejos.nxt.UltrasonicSensor;
import lejos.robotics.navigation.SteeringPilot;

public class TestSteeringPilot {
	static UltrasonicSensor capt= new UltrasonicSensor(SensorPort.S1);
	static TouchSensor touch = new TouchSensor(SensorPort.S2);
	static SteeringPilot sp = new SteeringPilot(0, Motor.A, false, Motor.B, 0, 0, 0);
	
	public static void capteADroite()  throws InterruptedException{
		Motor.C.rotate(45); 
		if(capt.getDistance()<=20)
		{
			Motor.C.rotate(-45);
			capteAGauche();
		}
		else
		{
			Motor.C.rotate(-45);
			sp.arcForward(45);
		}
	}
	
	public static void capteAGauche() throws InterruptedException{
		Motor.C.rotate(-45); 
		if(capt.getDistance()<=20)
		{

			 Motor.C.rotate(45);  
			 sp.arcBackward(10);
			 sp.travel(-100);
			 
			 //Motor.A.backward();
			 //Motor.B.backward();
			 //Thread.sleep (2000);
		}
		else
		{
			Motor.C.rotate(45); 
			sp.arcForward(-45);
		}
	}
	
	
	public static void main(String[] args) throws InterruptedException {
		sp.travel(1000);
		sp.setTravelSpeed(250);
		
		while(!Button.ESCAPE.isPressed())
			{
				LCD.drawString("GO!!" ,0,0 );
				LCD.refresh();
	 		
			  sp.forward();
	 		  
	 		  if(capt.getDistance()<=20)
	 		  {
	 			sp.stop();
	 			 capteADroite();
	 		 }
	 		 else if(touch.isPressed()==true)
	 		{
	 			sp.arcBackward(10);
	 			sp.travel(-100);
	 		}
		}
	}
}
